program Test_my_taylor
use my_own_da
use exercise_lecture_1
IMPLICIT NONE
type(my_taylor) L1,K1,L2,K2,x(2),L0,K0,xi(2)
!real(dp) L1,K1,L2,K2,x(2),L0,K0
INTEGER EXPONENTS(2),I,J,N,mf
REAL(DP) MATRIX(2,2),FRAC_PHASE,PI,FRAC_TUNE,matrix_inv(2,2)
REAL(DP) ALPHA0,BETA0,GAMMA0,ALPHA,BETA,GAMMA
type(my_taylor) COURANT_SNYDER
mf=16
open(unit=mf,file="twiss.txt")
100  FORMAT(2(I4,1x),3(F12.8,1x))
PI=ATAN(1.0_DP)*4.0_DP

N=10
L1=0.25D0
K1=20.0D0
L2=0.2D0
K2=-k1
L0=0.1d0
K0=0.D0

X(1)=0.D0;X(2)=0.D0;  !@1  Set the closed orbit to zero (we know what it is here)

X(1)=X(1) +(1.0_DP.mono.1)  !@1 <font color="#0000FF">X(1) = X<sub>0</sub> + x<sub>1</sub></font> 
X(2)=X(2) +(1.0_DP.mono.2)  !@1 <font color="#0000FF">X(2) = P<sub>0</sub> + x<sub>2</sub></font> 

DO I=1,10
 CALL QUAD_MY_TAYLOR(X,L1,K1,N) !@1
 CALL QUAD_MY_TAYLOR(X,L0,K0,N) !@1 <b><i><font color="#0000FF">10 Cells map computed here</font></i></b>
 CALL QUAD_MY_TAYLOR(X,L2,K2,N) !@1
 CALL QUAD_MY_TAYLOR(X,L0,K0,N) !@1
ENDDO

WRITE(6,*) " One turn Map for 10 FODO Cells "

WRITE(6,*) " FIRST RAY " 
WRITE(6,*) X(1)
WRITE(6,*) " SECOND RAY " 
WRITE(6,*) X(2)


WRITE(6,*) " LINEAR MATRIX IS "
  call get_matrix(x,MATRIX)   !@1 <font color="#0000FF">Our cheap routine to extract the linear part from the array X </font>
WRITE(6,*)  MATRIX(1,:)
WRITE(6,*)  MATRIX(2,:)

FRAC_PHASE=ACOS((MATRIX(1,1)+MATRIX(2,2))/2.D0)      !@1    
IF(MATRIX(1,2)<0.0_DP) FRAC_PHASE=-FRAC_PHASE+2*PI   !@1 <b><i><font color="#0000FF">Compute the fractional tune </font></i></b>
FRAC_TUNE=FRAC_PHASE/2/PI    !@1 
WRITE(6,*) FRAC_PHASE,FRAC_TUNE

BETA0 =  MATRIX(1,2)/SIN(FRAC_PHASE)  !@1  <i><font color="#0000FF">Compute <font face="Times New Roman">&#946;<sub>0</sub></font> &nbsp; from one-turn matrix </font></i>
GAMMA0= -MATRIX(2,1)/SIN(FRAC_PHASE)   !@1  <i><font color="#0000FF">Compute <font face="Times New Roman">&#947;<sub>0</sub></font>  &nbsp; from one-turn matrix </font></i>
ALPHA0= (MATRIX(1,1)-COS(FRAC_PHASE))/SIN(FRAC_PHASE) !@1  <i><font color="#0000FF">Compute <font face="Times New Roman">&#945;<sub>0</sub></font> &nbsp; from one-turn matrix </font></i>
WRITE(6,*) " GAMMA0,BETA0,2*ALPHA0 "
WRITE(6,*) GAMMA0,BETA0,2*ALPHA0
WRITE(6,*) (1.0_DP+ALPHA0**2)/BETA0/GAMMA0

COURANT_SNYDER=GAMMA0*X(1)**2+BETA0*X(2)**2+2.0_DP*ALPHA0*X(1)*X(2)  !@1 <b><i><font color="#0000FF">Compute</font></i></b><b><font color="#0000FF"><i> Courant-Snyder Invariant as a </i></font><i><font color="#FF00FF">MY_TAYLOR</font><font color="#0000FF">&nbsp; polynomial </font></i></b> 
WRITE(6,*) " CHECKING THE INVARIANCE OF THE COURANT-SNYDER INVARIANT " !@1 <b><i><font color="#0000FF">and print it.</font></i></b> 
WRITE(6,*) COURANT_SNYDER

! Using COURANT_SNYDER_i= COURANT_SNYDER_0 o M_0i^-1

! COMPUTING THE ONE TURN MAP
X(1)=0.D0;X(2)=0.D0;
X(1)=X(1) +(1.0_DP.mono.1)
X(2)=X(2) +(1.0_DP.mono.2)
DO I=1,10
                             call get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)   !@1 This routines computes the lattice function
                             write(mf,100) i,1,gamma,beta,alpha
 CALL QUAD_MY_TAYLOR(X,L1,K1,N)
                             call get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)    !@1 Click <a href="#GET_TWISS">here</a> to go to get_twiss 
                             write(mf,100) i,2,gamma,beta,alpha
 CALL QUAD_MY_TAYLOR(X,L0,K0,N)
                             call get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)
                             write(mf,100) i,3,gamma,beta,alpha
 CALL QUAD_MY_TAYLOR(X,L2,K2,N)
                             call get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)
                             write(mf,100) i,4,gamma,beta,alpha
 CALL QUAD_MY_TAYLOR(X,L0,K0,N)
ENDDO
                             call get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)
                             write(mf,100) 0,1,gamma,beta,alpha

close(mf)

end program Test_my_taylor


SUBROUTINE QUAD_MY_TAYLOR(X,L,K,N)
USE my_own_da
IMPLICIT NONE
INTEGER N,I
TYPE(my_taylor) K,L,X(2),DL
!real(dp) K,L,X(2),DL


DL=L/N

DO I=1,N
X(1)=X(1)+DL*X(2)/SQRT(1.D0-X(2)**2)  !@1 This is a simple first order drift-kick integrator
X(2)=X(2)-DL*K*X(1)
ENDDO


END SUBROUTINE QUAD_MY_TAYLOR


subroutine inv_matrix(mat,mati)
USE my_own_da
IMPLICIT NONE
real(dp) det,mat(2,2),mati(2,2)

det=mat(1,1)*mat(2,2)-mat(1,2)*mat(2,1)

mati(1,1)=mat(2,2)/det
mati(2,2)=mat(1,1)/det
mati(1,2)=-mat(1,2)/det
mati(2,1)=-mat(2,1)/det

end subroutine inv_matrix

subroutine get_matrix(x,MATRIX)
USE my_own_da
IMPLICIT NONE
type(my_taylor) x(2)
real(dp) MATRIX(2,2)
integer i,j,EXPONENTS(2)

DO I=1,2
DO J=1,2
 EXPONENTS=0
 EXPONENTS(J)=1
 MATRIX(I,J)=X(I).SUB.EXPONENTS
ENDDO
ENDDO

end subroutine get_matrix

subroutine make_rays_with_matrix(MATRIX,x)
USE my_own_da
IMPLICIT NONE
type(my_taylor) x(2),x_1,x_2
real(dp) MATRIX(2,2)

x_1=1.0_dp.mono.1
x_2=1.0_dp.mono.2

x(1)=MATRIX(1,1)*x_1+MATRIX(1,2)*x_2
x(2)=MATRIX(2,1)*x_1+MATRIX(2,2)*x_2


end subroutine make_rays_with_matrix 

subroutine get_twiss(x,gamma0,beta0,alpha0,gamma,beta,alpha)
USE my_own_da
IMPLICIT NONE
type(my_taylor) x(2)
real(dp) gamma0,beta0,alpha0,gamma,beta,alpha
type(my_taylor) COURANT_SNYDER,xi(2)
real(dp) MATRIX(2,2),MATRIX_inv(2,2)
integer EXPONENTS(2)

!  Get lattice functions
 call get_matrix(x,MATRIX)         !@1 X(2) contains the nonlinear map from 0 to i. The linear part is put matrix(2,2)
 call inv_matrix(MATRIX,MATRIX_inv) !@1  matrix  is inverted into matrix_inv 
 call make_rays_with_matrix(MATRIX_inv,xi) !@1  matrix_inv  is converted into two first degree polynomials, <font face="Times New Roman"><font size="5">xi = M<sub>0i</sub><sup>-1</sup></font><b>X</b><font size="5"></font></font>
 COURANT_SNYDER=GAMMA0*Xi(1)**2+BETA0*Xi(2)**2+2.0_DP*ALPHA0*Xi(1)*Xi(2)  !@1 <font face="Times New Roman"><font size="5">&#949;</font><sup>1<font size="5">(</font></sup><b> X</b><sup> <font size="5">&nbsp;)</font></sup>= <font size="5">&#949;</font><sup>0 </sup><font size="5">(M<sub>0i</sub><sup>-1</sup></font><b>X</b><font size="5">  )</font></font>
 EXPONENTS(1)=2;EXPONENTS(2)=0;
 gamma=COURANT_SNYDER.sub.EXPONENTS
 EXPONENTS(1)=0;EXPONENTS(2)=2;
 beta=COURANT_SNYDER.sub.EXPONENTS
 EXPONENTS(1)=1;EXPONENTS(2)=1;
 alpha=(COURANT_SNYDER.sub.EXPONENTS)/2.0_dp


end subroutine get_twiss 

